#include "modbus.h"
#include <string.h>
#include <stdlib.h>
#include "stdcrc16.h"
#include "com.h"
#include "dev_api.h"

#define MODBUS_DEVICE_ID        0x01
#define MODBUS_DATA_SIZE        254
#define MODBUS_BUFFER_SIZE      512
#define MODBUS_FUNCTION_03H     0x03
#define MODBUS_FUNCTION_10H     0x10
#define MODBUS_FUNCTION_ERROR   0x80
#define MODBUS_REG_FIELD_START  RAM_FIELD_START
#define MODBUS_RESERVED         RAM_FIELD_END

#define DEBUG(...)

typedef struct modbus_frame_s {
  uint8_t data[MODBUS_DATA_SIZE];
  uint8_t ID;
  uint8_t Function;
  uint16_t Starting_Address;
  uint16_t Num_of_Points;
  uint8_t Bytes_Count;
} modbus_frame_t;

static int32_t modbus_read(uint16_t reg, uint8_t *pbuf, int32_t points);
static int32_t modbus_write(uint16_t reg, uint8_t *pbuf, int32_t points);

static void swap16(uint16_t *data)
{
  uint16_t temp;
  temp = *data;
  *data = (uint16_t)((temp>>8)|(temp<<8));
}

static void cache_swap16(uint8_t* pbuf, int32_t size)
{
  int32_t i;
  uint16_t* pdata;
  pdata = (uint16_t*)pbuf;
  for(i=0;i<(size>>1);i++) {
    swap16(&pdata[i]);
  }
}

static int32_t modbus_check(uint8_t *pbuf, int32_t size)
{
  int32_t offset = 0;
  uint16_t cal_crc16, frame_crc16;
  if(size<8) return EXIT_FAILURE;
  for(offset=0;offset<=(size-8);offset++) {
    if(pbuf[offset] != MODBUS_DEVICE_ID) continue;
    if(pbuf[offset+1] == MODBUS_FUNCTION_03H) {
      if((size-offset) < 8) continue;
      memcpy(&frame_crc16, &pbuf[offset+6], 2);
      swap16(&frame_crc16);
      cal_crc16 = CRC16_MODBUS(&pbuf[offset], 6);
      if(cal_crc16 != frame_crc16) continue;
      if(offset) memmove(&pbuf[0], &pbuf[offset], offset);
      return EXIT_SUCCESS;
    }
    else if(pbuf[offset+1] == MODBUS_FUNCTION_10H) {
      if((size-offset) < 11) continue;
      memcpy(&frame_crc16, &pbuf[offset+7+pbuf[offset+6]], 2);
      swap16(&frame_crc16);
      cal_crc16 = CRC16_MODBUS(&pbuf[offset], 7+pbuf[offset+6]);
      if(cal_crc16 != frame_crc16) continue;
      if(offset) memmove(&pbuf[0], &pbuf[offset], offset);
      return EXIT_SUCCESS;
    }
  }
  return EXIT_FAILURE;
}

static int32_t modbus_parse(void* frame, uint8_t* pbuf, int32_t size)
{
  modbus_frame_t* p_frame;
  if(frame == NULL) return EXIT_FAILURE;
  if(pbuf == NULL) return EXIT_FAILURE;
  p_frame = (modbus_frame_t*)frame;
  p_frame->ID = pbuf[0];
  p_frame->Function = pbuf[1];
  if(p_frame->Function == MODBUS_FUNCTION_03H) {
    memcpy(&p_frame->Starting_Address, &pbuf[2], 2);
    swap16(&p_frame->Starting_Address);
    memcpy(&p_frame->Num_of_Points, &pbuf[4], 2);
    swap16(&p_frame->Num_of_Points);
    return EXIT_SUCCESS;
  }
  else if(p_frame->Function == MODBUS_FUNCTION_10H) {
    memcpy(&p_frame->Starting_Address, &pbuf[2], 2);
    swap16(&p_frame->Starting_Address);
    memcpy(&p_frame->Num_of_Points, &pbuf[4], 2);
    swap16(&p_frame->Num_of_Points);
    p_frame->Bytes_Count = pbuf[6];
    DEBUG("%04X %04X %d\n", p_frame->Starting_Address, p_frame->Num_of_Points, p_frame->Bytes_Count);
    if(p_frame->Bytes_Count != (p_frame->Num_of_Points*2)) return EXIT_FAILURE;
    if(p_frame->Bytes_Count == 0) return EXIT_FAILURE;
    if(p_frame->Bytes_Count > size) return EXIT_FAILURE;
    memcpy(p_frame->data, &pbuf[7], p_frame->Bytes_Count);
    cache_swap16(p_frame->data, p_frame->Bytes_Count);
    return EXIT_SUCCESS;
  }
  return EXIT_FAILURE;
}

static int32_t modbus_make(void* frame, uint8_t* pbuf, int32_t size)
{
  modbus_frame_t* p_frame;
  uint16_t cal_crc16;
  if(frame == NULL) return 0;
  if(pbuf == NULL) return 0;
  p_frame = (modbus_frame_t*)frame;
  if(p_frame->Function & MODBUS_FUNCTION_ERROR) {
    pbuf[0] = p_frame->ID;
    pbuf[1] = p_frame->Function;
    pbuf[2] = 0x01;
    cal_crc16 = CRC16_MODBUS(&pbuf[0], 3);
    swap16(&cal_crc16);
    memcpy(&pbuf[3], &cal_crc16, 2);
    return 5;
  }
  else if(p_frame->Function == MODBUS_FUNCTION_03H) {
    if(size<(p_frame->Bytes_Count+5)) return 0;
    pbuf[0] = p_frame->ID;
    pbuf[1] = p_frame->Function;
    pbuf[2] = p_frame->Bytes_Count;
    cache_swap16(p_frame->data, p_frame->Bytes_Count);
    memcpy(&pbuf[3], p_frame->data, p_frame->Bytes_Count);
    cal_crc16 = CRC16_MODBUS(&pbuf[0], p_frame->Bytes_Count+3);
    swap16(&cal_crc16);
    memcpy(&pbuf[p_frame->Bytes_Count+3], &cal_crc16, 2);
    return p_frame->Bytes_Count+5;
  }
  else if(p_frame->Function == MODBUS_FUNCTION_10H) {
    pbuf[0] = p_frame->ID;
    pbuf[1] = p_frame->Function;
    swap16(&p_frame->Starting_Address);
    memcpy(&pbuf[2], &p_frame->Starting_Address, 2);
    swap16(&p_frame->Num_of_Points);
    memcpy(&pbuf[4], &p_frame->Num_of_Points, 2);
    cal_crc16 = CRC16_MODBUS(&pbuf[0], 6);
    swap16(&cal_crc16);
    memcpy(&pbuf[6], &cal_crc16, 2);
    return 8;
  }
  return 0;
}

int32_t modbus_service(uint8_t* pbuf, int32_t size, uint32_t com_handler)
{
  modbus_frame_t frame;
  int32_t write_size = 0;
  com_dev_t *p_com = (com_dev_t *)com_handler;
  if(pbuf == NULL) return EXIT_FAILURE;
  DEBUG("<modbus_service>\n");
  if(modbus_check(pbuf, size)) {
    DEBUG("return => modbus_check\n");
    return EXIT_FAILURE;
  }
  if(modbus_parse(&frame, pbuf, MODBUS_DATA_SIZE)) {
    DEBUG("return => modbus_parse\n");
    return EXIT_FAILURE;
  }
  DEBUG("function = 0x%02X\n", frame.Function);
  DEBUG("Starting_Address = 0x%04X\n", frame.Starting_Address);
  DEBUG("Num_of_Points = 0x%04X\n", frame.Num_of_Points);
  if(frame.Function == MODBUS_FUNCTION_03H) {
    frame.Bytes_Count = modbus_read(frame.Starting_Address, frame.data, frame.Num_of_Points);
    if(frame.Bytes_Count)
      write_size = modbus_make(&frame, pbuf, UART_CACHE_SIZE);
  }
  else if(frame.Function == MODBUS_FUNCTION_10H) {
    if(frame.Bytes_Count == modbus_write(frame.Starting_Address, frame.data, frame.Num_of_Points))
      write_size = modbus_make(&frame, pbuf, UART_CACHE_SIZE);
  }
  if(write_size == 0) {
    frame.Function |= MODBUS_FUNCTION_ERROR;
    write_size = modbus_make(&frame, pbuf, UART_CACHE_SIZE);
  }
  p_com->write(pbuf, write_size);
  return EXIT_SUCCESS;
}

static int32_t modbus_read(uint16_t reg, uint8_t *pbuf, int32_t points)
{
  uint32_t address;
  if(((reg + points) << 1) >= (MODBUS_RESERVED-MODBUS_REG_FIELD_START)) return 0;
  address = MODBUS_REG_FIELD_START + (reg << 1);
  return ram_read(address, pbuf, points<<1);
}

static int32_t modbus_write(uint16_t reg, uint8_t *pbuf, int32_t points)
{
  uint32_t address;
  if(((reg + points) << 1) >= (MODBUS_RESERVED-MODBUS_REG_FIELD_START)) return 0;
  address = MODBUS_REG_FIELD_START + (reg << 1);
  return ram_write(address, pbuf, points<<1);
}

